#include "ParticleSystem.h"

void ParticleSystem::Simulation() {

	BT_PROFILE("ParticleSystemSimulation");

	float residualDt = timeStepSize;
	while (residualDt > 0.0f) {
		if (residualDt < maxSubTimeStepSize) dt = residualDt;
		else dt = maxSubTimeStepSize;
		residualDt -= dt;
		Advection();
		Neighboring();
		ComputeRho();
		ComputeForceAndVelocity();
	}
}

void ParticleSystem::renderParticle() {

	BT_PROFILE("RenderParticles");

	glPushMatrix();
	glPushAttrib( GL_POLYGON_BIT ); // includes GL_CULL_FACE
	glDisable(GL_CULL_FACE); // draw from all sides
	glPointSize(6.0f);
	

	glBegin(GL_POINTS);
	for (int i=0; i<numParticle; i++) {
		glColor3f(pos[i].x(), pos[i].y(), 1.0);
		Vector3 position = pos[i] * scalingFactor;
		glVertex3f(position.x(), position.y(), position.z());
		
	}
	glEnd();

	glPopAttrib(); // GL_CULL_FACE
	glPopMatrix();
}

void ParticleSystem::ParticleSeeding() {

	Vector3 startPos = Vector3(0.0, 0.15, 0.0);
	float gap = h * 0.6;
	//float gap = 1.0f / 32.0f;
	int cnt = 0;
	int sline = pow(numParticle, 1./3.) +1;
	sline = min(sline, (int)1. / gap);
	for (int i=0; i<sline; i++) {
		for (int j=0; j<sline; j++) {
			for (int k=0; k<sline; k++) {
				Vector3 tmpPos, tmpVel;
				tmpPos = startPos + Vector3(j, i, k) * gap;
				tmpVel = Vector3(0.0, 0.0, 0.0);
				pos.push_back(tmpPos);
				vel.push_back(tmpVel);
				mm.push_back(Vector3(0.0, 0.0, 0.0));
				rho.push_back(0.0);
				pressure.push_back(0.0);

				cnt++;
				if (cnt == numParticle) return;
			}
		}
	}
	numParticle=cnt;
}

void ParticleSystem::Advection() {

	BT_PROFILE("Advection");

	gridMap.Clear();

	for (int i=0; i<numParticle; i++) {
		Vector3 prePos = pos[i];
		pos[i] += vel[i]*dt;

		DetectCollision(prePos, pos[i], vel[i]);

		SpatialGrid(pos[i], i);
	}
}

void ParticleSystem::DetectCollision(Vector3& prePos, Vector3& pos, Vector3& vel) {
	
	BT_PROFILE("DetectCollision");

	btVector3 worldPrePos = ParticleSystemPosition + prePos * scalingFactor;
	btVector3 worldPos = ParticleSystemPosition + pos * scalingFactor;
	btVector3 vel_ = vel * scalingFactor;

	btVector3 start = worldPrePos;
	btVector3 end = worldPos;

	btCollisionWorld::ClosestRayResultCallback cb(start, end);
	m_dynamicsWorld->rayTest(start, end, cb);

	if (cb.hasHit()) {
		Vector3 hitPoint = cb.m_hitPointWorld / scalingFactor;
		Vector3 hitNormal = cb.m_hitNormalWorld.normalized();
		if ( (prePos-hitPoint).dot(hitNormal) * (pos-hitPoint).dot(hitNormal) <= 0.0f) {
			pos = hitPoint + hitNormal*0.0001f;
			if ( vel.dot(hitNormal) <= 0.0f) {
				vel -= vel.dot(hitNormal) * hitNormal;
			}
		}
	}

	if (pos.x() < 0.0) {pos.x() = 0.0; vel.x() = 0.0;}
	if (pos.y() < 0.0) {pos.y() = 0.0; vel.y() = 0.0;}
	if (pos.z() < 0.0) {pos.z() = 0.0; vel.z() = 0.0;}

	if (pos.x() > domain.x()) {pos.x() = domain.x(); vel.x() = 0.0;}
	if (pos.y() > domain.y()) {pos.y() = domain.y(); vel.y() = 0.0;}
	if (pos.z() > domain.z()) {pos.z() = domain.z(); vel.z() = 0.0;}

}

void ParticleSystem::SpatialGrid(Vector3& pos, int index) {
	BT_PROFILE("SpatialGrid");
	gridMap.Add(GridIndex(pos/h), index);
	
}

void ParticleSystem::Neighboring() {

	BT_PROFILE("Neighboring");

	neighborMap.Clear();
	
	for (int i=0; i<numParticle; i++) {
		int x = (int) pos[i].x()/h;
		int z = (int) pos[i].z()/h;

		if (x>=numXgrid) x = numXgrid - 1;
		if (z>=numZgrid) z = numZgrid - 1;

		for (int dx=-1; dx<2; dx++) {
			int nx = x+dx;
			if (nx<0 || nx>=numXgrid) continue;
			for (int dz=-1; dz<2; dz++) {
				int nz = z+dz;
				if (nz<0 || nz>=numZgrid) continue;
				int index = GridIndex(nx, nz);
				if (!gridMap.isInserted(index)) continue;
				vector<int> cell = gridMap[index];
				int cnt = cell.size();
				for (int ci=0; ci<cnt; ci++) {
					int neighborIndex = cell[ci];
					if (i==neighborIndex) continue;
					float distance = (pos[i] - pos[neighborIndex]).length();
					if (distance < h) neighborMap.Add(i, neighborIndex);
				}
			}
		}
	}
}

void ParticleSystem::ComputeRho() {

	BT_PROFILE("ComputeRho");

	for (int i=0; i<numParticle; i++) {
		Vector3 pi = pos[i];
		float tempRho = pow(h,6);
		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors; ci++) {
				Vector3 pj = pos[neighborOfi[ci]];
				float distance = (pi-pj).length();
				tempRho += pow(h*h-distance*distance, 3);
			}
		}
		tempRho *= m* constantOfPoly6;
		if (tempRho < rho0*0.8f) {
			tempRho = rho0;
		}
		else if (tempRho < rho0) {
			tempRho = rho0*sigma;
		}
		rho[i] = tempRho;
		pressure[i] = k * (rho[i] - rho0);
	}
}

void ParticleSystem::ComputeForceAndVelocity() {
	BT_PROFILE("ComputeForceAndVelocity");

	for (int i=0; i<numParticle; i++) {
		Vector3 posi = pos[i];
		Vector3 veli = vel[i];
		Vector3 pressureForce;
		Vector3 viscosityForce;
		float rhoi = rho[i];
		float pressurei = pressure[i];

		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors; ci++) {
				Vector3 posj = pos[neighborOfi[ci]];
				Vector3 velj = vel[neighborOfi[ci]];
				float rhoj = rho[neighborOfi[ci]];
				float pressurej = pressure[neighborOfi[ci]];
				Vector3 r = posi - posj;
				float distance = r.length();
				pressureForce -= ((pressurei + pressurej) / (2.0f * rhoj)) * (float) pow(h-distance,2) * (r/distance);
				viscosityForce += (velj - veli) * (h-distance);
			}
		}
		pressureForce *= m*constantOfGradientSpiky;
		viscosityForce*=(mu/rhoi)*m*constantOfLaplacianViscosity;

		externalForce = gravity;

		Vector3 force = pressureForce + viscosityForce+ rhoi*externalForce;
		Vector3 acceleration = force / rhoi;
		vel[i] += acceleration * dt;
		/*
		if (vel[i].length() > 5.0f) {
			vel[i]
		}*/

		vel[i] *= 0.999f;
	}
}

int ParticleSystem::GridIndex(const Vector3& v) {
	BT_PROFILE("GridIndex");
	return GridIndex((int)v.x(), (int)v.z());
}
int ParticleSystem::GridIndex(int x, int z) {
	BT_PROFILE("GridIndex");
	if (x>=numXgrid) x=numXgrid-1;
	if (z>=numZgrid) z=numZgrid-1;
	return x + numXgrid * z;
}